/*============================================================================*/
/*                          The Freescale Cup                                 */
/*============================================================================*/
/*                        OBJECT SPECIFICATION                                */
/*============================================================================*
* C Source:         apptask.c%
* %version:         1 %
* %created_by:      Continental %
* %date_created:     %
*=============================================================================*/
/* DESCRIPTION :  All tasks are called here                                   */
/*============================================================================*/
/* FUNCTION COMMENT : Add here your tasks, inside a periodic task             */
/* according to the desired execution period                                  */
/*                                                                            */
/*============================================================================*/
/*                               OBJECT HISTORY                               */
/*============================================================================*/
/*  REVISION |   DATE      |                               |      AUTHOR      */
/*----------------------------------------------------------------------------*/
/*  1.0      | 22/08/2011  |  Revision                     | Continental      */
/*                                                                            */
/*============================================================================*/

/* Includes */
/* ------------------- */
/* Common declarations */
/* ------------------- */
#include "apptask.h"

/*       SW API    */
/* --------------- */
#include "APPLeds.h"

#include "HALLeds.h"

/* Exported functions prototypes */
/* ----------------------------- */
void apptask_28us(void);
void apptask_28usd(void);
void apptask_56us(void);
void apptask_56us(void);
void apptask_pxRead(void);
void apptask_1ms(void);
void apptask_14ms(void);



/*Variables */
static T_ULONG pixels_camara[128];
static T_UBYTE LineArray[5];
bool capture_pixel= FALSE;
static T_UBYTE POSTN;

static T_ULONG minimo=0;

 T_UBYTE velocity1 = 100;    	//90 14 % ancho de pulso 
 T_UBYTE mDerecho = 100;//60
 T_UBYTE mIzquierdo = 100;//60
 
static T_UBYTE 	direction=11; //usada para formula  
static T_UWORD direction1 = 611;		//  611 90 grados pocision del servo
   	//490turn-left  720 turn-right
 
	bool i= FALSE;//counter for velocity control 
	bool run= TRUE;
 //T_ULONG RESUL_ADC1=0;
 
/**************************************************************
 *  Name                 : apptask_28us
 *	ID					 : TASK_28US
 *  Description          : 
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/

 void apptask_28us(void)
 {

	/*
	//VELOCITY CONTROL
	if(i<4){
		LineArray[i]=(T_UBYTE)(POSTN);
		i++;
	}
	if(i==4)
	{
		controlVelocity();	
		i=0;
	}
	initEMIOS_0chX(pin_PA9, direction1);	*/
		 			
 }
 
 /**************************************************************
 *  Name                 : apptask_28usd
 *	ID					 : TASK_1MS
 *  Description          : SI CAMARA
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
//cambiar metodo de si y ck en un metodo de integracion para estos dos puertos 
 void apptask_28usd(void) {
		
	static T_UWORD lub_28us_counter =1;//declarar fuera del metodo 
 	inicio: 	
 	
 	/*Run every 40 ms (20ms task x 2)*/
 	if((lub_28us_counter ==2)||(lub_28us_counter ==3)){
 	 	
 	 		/*Toggle Led 1 every 40ms*/
 	 	
 		//APPLeds_Led1_app();
 		Set_Led1_State(LED_ON);
 	}
 	else{
 			Set_Led1_State(LED_OFF);
 	}
 	
 	if(lub_28us_counter == 599){
 		Set_Led1_State(LED_OFF);
 		lub_28us_counter= 1;
 		goto inicio;
 	}
 	
		 
	lub_28us_counter++;
 }
 
   /**************************************************************
 *  Name                 : apptask_56us
 *	ID					 : TASK_56US
 *  Description          :Signal CK CAMARA
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_56us(void){
 	
 	static T_UWORD lub_28us_counter1 =1;
 	inicio: 	
 	
 	/*Run every 40 ms (20ms task x 2)*/
 	
 	if((lub_28us_counter1!=1)&&(lub_28us_counter1<=258)){
 	
 		/*Toggle Led 1 every 40ms*/
 		APPLeds_Led2_app();
 	}
 	else{
 			Set_Led2_State(LED_OFF);
 	}
 	
 	if(lub_28us_counter1 == 300){
 	
 		lub_28us_counter1= 1;
 		goto inicio;
 	}
 	
		 
	lub_28us_counter1++;
 
 
 

 }
 
    /**************************************************************
 *  Name                 : apptask_112us
 *	ID					 : TASK_112US
 *  Description          :pixel read
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_pxRead(void){
 //128 pixeles por leer 
 //total de cuentas 150
	static T_UWORD pixel_112us_counter =1;//incrementa la posicion del pixel 
	static T_UWORD pixel_array_counter =0;//utilizado para el arreglo 
 	inicio: 	
 	
 	/****************/
 	//RESUL_ADC1=(T_ULONG)(Read_adcMotor_State());
 	
 	
	/*if(Read_adcMotor_State()>=1300)
	{
		velocity1=170;
	}*/
	
	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 	if((pixel_112us_counter!=1)&&(pixel_112us_counter<=258)&&((pixel_112us_counter%2)==0)){
 	
 		pixels_camara[pixel_array_counter]=(T_ULONG)(Read_adc_State());
 		pixel_array_counter++;
 	}
 	if( pixel_112us_counter ==259){
			 capture_pixel= TRUE;		 		
 	}
 	
 	if(pixel_112us_counter == 300){
 	
 		pixel_112us_counter= 1;
 		pixel_array_counter =0;
 		goto inicio;
 	}
 	
		 
	pixel_112us_counter++;
 

 		

 }
 
 
  
/**************************************************************
 *  Name                 : apptask_5ms
 *	ID					 : TASK_5MS
 *  Description          : Contol to the velocity for independing wile 
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_1ms(void)
 {
 
 		if((POSTN>=42)&&(POSTN<=83)){
 			if(i==FALSE){
 				i=TRUE;
 				mDerecho= velocity1;
 				mIzquierdo= velocity1;	
 			}
 			//
 			/*	if(i==TRUE){
 				
 				mDerecho= mDerecho+10;
 				mIzquierdo= mIzquierdo+10;	
 			}*/
 		 	if(mIzquierdo<160)
 		 	{
 		 	
 				mIzquierdo+=5;
 				mDerecho+=5;	
 		 	} 	
 			
 		}
 		else if((POSTN>=0)&&(POSTN<=41))	{
 			mDerecho= 100;
 			mIzquierdo=65;	
 			i=FALSE;
 			
 		}
 		else if((POSTN>=84)&&(POSTN<=127)){
 			mDerecho= POSTN;//cambiar
 			mIzquierdo=100;
 			i=FALSE;
 		}
 
		initEMIOS_0chX(pin_PA6, mIzquierdo);//motor derecho 
		initEMIOS_0chX(pin_PA7, mDerecho);//motor izquierdo
				
		
	 }
 
 
 
 /**************************************************************
 *  Name                 : apptask_14ms
 *	ID					 : TASK_14MS
 *  Description          :
 *  Parameters           :  none
 *  Return               :	none
 *  Critical/explanation :  no
 **************************************************************/
 void apptask_14ms(void){
 
 static T_UBYTE y=0;
 minimo= pixels_camara[5];
 
 
 	while((capture_pixel== TRUE)&&(run== TRUE))
 	{
 		IO_PinWrite(pin_PH1, ONE);
 		IO_PinWrite(pin_PH3, ONE);
 	}
 	if((capture_pixel== TRUE)&&(capture_pixel== TRUE))
 	{
 		if((pixels_camara[59]<750)&&(pixels_camara[64]<750)&&(pixels_camara[71]<750))
 		{
 			
 			IO_PinWrite(pin_PH1, ZERO);
 			IO_PinWrite(pin_PH3, ZERO);
 			run=FALSE;
 		}
 	}
 
 
  
 
 	if(capture_pixel== TRUE){
		T_UBYTE x= 5;
		for(x; x<122;x++)//127
		{
		
		
			if(pixels_camara[x]<minimo){
				
				minimo=pixels_camara[x];
				POSTN=x;
			}

 		}
		
			
 	}
 //camviar para el concurso
 	
 	
 	//setservo(POSTN);
	initEMIOS_0chX(pin_PA9, direction1);
 	
 	
}


